#ifndef __ODOM_H
#define __ODOM_H

#include "stm32f4xx.h"
#include "data_holder_ll.h"
#include <stdbool.h>

#ifndef PI
#define PI 3.141592654
#endif

#ifdef __cplusplus
 extern "C" {
#endif
     
struct Pose_T{
    double x;
    double y;
    double th;                 //角度(度)
};

struct Twist_T{
    double vx;
    double vy;                  //线速度(m/s)
    double vth;                 //角速度(rad/s)
};

struct Odom_T{
    struct Pose_T pose;
    struct Twist_T twist;
};

struct MotorVels_T{
    short vel[8];
    short wheel_vel;
};

void odom_sum(SetDoubleCar_T team_, struct MotorVels_T motor_, WheelState_T wheel_state, float* true_angles, float* keep_angles);
void odom_clear(void);
float rpm2mps(short rpm);
short mps2rpm(float mps);
     
typedef struct{
    struct Odom_T odom;
    u32 ms_tick;                //用于计算时间间隔的计时器
    double last_yaw;            //上一次的偏角
    double current_yaw;         //当前偏角
    void (*sum)(SetDoubleCar_T team_, struct MotorVels_T motor_, WheelState_T wheel_state, float* true_angles, float* keep_angles);
    void (*clear)(void);
}Odometry_T;

extern Odometry_T Odometry;
     
#ifdef __cplusplus
 }
#endif

#endif
